package Rangel.ambulance.detector.distancedased;

import adf.agent.info.AgentInfo;
import adf.agent.info.ScenarioInfo;
import adf.agent.info.WorldInfo;
import adf.agent.module.ModuleManager;
import adf.component.module.algorithm.Clustering;
import adf.component.module.algorithm.PathPlanning;
import Rangel.common.world.tools.Util;
import rescuecore2.misc.Pair;
import rescuecore2.standard.entities.*;
import rescuecore2.worldmodel.EntityID;
import java.util.*;
import static rescuecore2.standard.entities.StandardEntityURN.REFUGE;

public class DistanceBasedTargetSelector {

    private Map<EntityID, AmbulanceTarget> targetsMap;
    private int rescueRange;
    protected boolean isMapHuge = false;
    protected boolean isMapMedium = false;
    protected boolean isMapSmall = false;
    public static final double MEAN_VELOCITY_OF_MOVING = 31445.392;
    protected int minX, minY, maxX, maxY;
    private AmbulanceTarget previousTarget = null;
    private int clusterIndex;
    Map<Integer, Pair<Integer, Integer>> clusterCenterMap;

    private WorldInfo worldInfo;
    private ScenarioInfo scenarioInfo;
    private AgentInfo agentInfo;
    private Clustering clustering;
    private PathPlanning pathPlanning;

    public DistanceBasedTargetSelector(AgentInfo ai, WorldInfo wi, ScenarioInfo si, ModuleManager moduleManager) {

        this.worldInfo = wi;
        this.scenarioInfo = si;
        this.agentInfo = ai;

        switch (scenarioInfo.getMode()) {
            case PRECOMPUTATION_PHASE:
            case PRECOMPUTED:
            case NON_PRECOMPUTE:
                this.pathPlanning = moduleManager.getModule("SampleRoadDetector.PathPlanning", "adf.sample.module.algorithm.SamplePathPlanning");
                this.clustering = moduleManager.getModule("SampleVictimSelector.Clustering", "adf.sample.module.algorithm.SampleKMeans");
                break;
        }

        targetsMap = new HashMap<>();
        clusterCenterMap = new HashMap<>();
        calculateMapDimensions();
        verifyMap();
        initializeRescueRange();
    }


    private void calculateMapDimensions() {
        this.minX = Integer.MAX_VALUE;
        this.maxX = Integer.MIN_VALUE;
        this.minY = Integer.MAX_VALUE;
        this.maxY = Integer.MIN_VALUE;
        Pair<Integer, Integer> pos;
        for (StandardEntity standardEntity : worldInfo.getAllEntities()) {
            pos = standardEntity.getLocation(worldInfo.getRawWorld());
            if (pos.first() == Integer.MIN_VALUE || pos.first() == Integer.MAX_VALUE || pos.second() == Integer.MIN_VALUE || pos.second() == Integer.MAX_VALUE) {
                continue;
            }
            if (pos.first() < this.minX)
                this.minX = pos.first();
            if (pos.second() < this.minY)
                this.minY = pos.second();
            if (pos.first() > this.maxX)
                this.maxX = pos.first();
            if (pos.second() > this.maxY)
                this.maxY = pos.second();
        }
    }

    private int getMapWidth() {
        return maxX - minX;
    }

    private int getMapHeight() {
        return maxY - minY;
    }


    private void verifyMap() {

        double mapDimension = Math.hypot(getMapWidth(), getMapHeight());

        double rate = mapDimension / MEAN_VELOCITY_OF_MOVING;

        if (rate > 60) {
            isMapHuge = true;
        } else if (rate > 30) {
            isMapMedium = true;
        } else {
            isMapSmall = true;
        }


    }

    private void initializeRescueRange() {
        int perceptionLosMaxDistance = scenarioInfo.getPerceptionLosMaxDistance();
        if (isMapSmall) {
            rescueRange = perceptionLosMaxDistance * 2;
        } else if (isMapMedium) {
            rescueRange = perceptionLosMaxDistance * 4;
        } else {
            rescueRange = perceptionLosMaxDistance * 6;
        }
    }

    public EntityID nextTarget(Set<StandardEntity> victims) {
        this.clusterIndex = clustering.getClusterIndex(this.agentInfo.getID());
        Collection<StandardEntity> elements = clustering.getClusterEntities(this.clusterIndex);

        calculateMapCenters(this.clusterIndex, elements);


        targetsMap.clear();

        if (previousTarget != null && !victims.contains(worldInfo.getEntity(previousTarget.getVictimID()))) {
            previousTarget = null;
        }


        refreshTargetsMap(victims, targetsMap);

        calculateDecisionParameters(victims, targetsMap);

        calculateVictimsCostValue();
        RangelNewCostsCalculation();


        AmbulanceTarget bestTarget;
        bestTarget = findBestVictim(targetsMap, elements);
        if (previousTarget != null && victims.contains(worldInfo.getEntity(previousTarget.getVictimID()))) {
            if (bestTarget != null && !bestTarget.getVictimID().equals(previousTarget.getVictimID())) {
                Human bestHuman = (Human) worldInfo.getEntity(bestTarget.getVictimID());
                Human previousHuman = (Human) worldInfo.getEntity(previousTarget.getVictimID());

                double bestHumanCost = pathPlanning.setFrom(agentInfo.getPosition()).setDestination(bestHuman.getPosition()).calc().getDistance();
                double previousHumanCost = pathPlanning.setFrom(agentInfo.getPosition()).setDestination(previousHuman.getPosition()).calc().getDistance();
                if (previousHumanCost < bestHumanCost) {
                    bestTarget = previousTarget;
                }
            }
        }

        previousTarget = bestTarget;

        if (bestTarget != null) {
            return bestTarget.getVictimID();
        } else {
            return null;
        }

    }

    private void calculateMapCenters(int clusterIndex, Collection<StandardEntity> elements) {
        int minX = Integer.MAX_VALUE, minY = Integer.MAX_VALUE;
        int maxX = 0, maxY = 0;
        for (StandardEntity entity : elements) {
            Pair<Integer, Integer> location = worldInfo.getLocation(entity);
            minX = Math.min(location.first(), minX);
            minY = Math.min(location.second(), minY);

            maxX = Math.max(location.first(), maxX);
            maxY = Math.max(location.second(), maxY);
        }

        Pair<Integer, Integer> center = new Pair<>((minX + maxX) / 2, (minY + maxY) / 2);
        clusterCenterMap.put(clusterIndex, center);

    }

    private void refreshTargetsMap(Set<StandardEntity> victims, Map<EntityID, AmbulanceTarget> targetsMap) {
        List<EntityID> toRemoves = new ArrayList<>();
        for (EntityID targetID : targetsMap.keySet()) {
            if (!victims.contains(worldInfo.getEntity(targetID))) {
                toRemoves.add(targetID);
            }
        }
        for (EntityID entityID : toRemoves) {
            targetsMap.remove(entityID);
        }
    }

    private AmbulanceTarget findBestVictim(Map<EntityID, AmbulanceTarget> targetsMap, Collection<StandardEntity> elements) {
        AmbulanceTarget bestTarget = null;
        if (targetsMap != null && !targetsMap.isEmpty()) {

            double minValue = Double.MAX_VALUE;
            for (AmbulanceTarget target : targetsMap.values()) {
                if (target.getDistanceToMe() <= rescueRange || elements.contains(worldInfo.getEntity(target.getPositionID()))) {
                    if (target.getDistanceToMe() < minValue) {
                        minValue = target.getDistanceToMe();
                        bestTarget = target;
                    }
                }
            }

        }

        return bestTarget;
    }
    private void RangelNewCostsCalculation() {
        if (targetsMap != null && !targetsMap.isEmpty()) {
            double cost = 0;
            for (StandardEntity next : worldInfo.getEntitiesOfType(
                    StandardEntityURN.CIVILIAN
            )
            ) {
                Human h = (Human) next;
               double brd=h.getBuriedness();
               if(brd==0){
                   continue;
               }
               if(brd>=1&&brd<10){
                   cost=1.2;
               }
                if(brd>=10&&brd<20){
                    cost=1.3;
                }
                if(brd>=20&&brd<30){
                    cost=1.4;
                }
                if(brd>=30&&brd<40){
                    cost=1.5;
                }

                if(brd>=40&&brd<50){
                    cost=1.6;
                }


                if(brd>=50&&brd<61){
                    cost=1.1;
                }



                for (AmbulanceTarget target : targetsMap.values()) {
                    target.setCost(cost);
                }

            }
        }
    }
    private void calculateVictimsCostValue(){

        if (targetsMap != null && !targetsMap.isEmpty()) {
            double cost;
            double rw = .9;//refuge Weight
            double pdw = 2.7;//my Partition Distance Weight
            double mdw = 1.5;//My Distance Weight
            double vsw = 1.5; //Victim Situation Weight
            for (AmbulanceTarget target : targetsMap.values()) {
                cost = 1 + rw * target.getDistanceToRefuge() / MEAN_VELOCITY_OF_MOVING
                        + pdw * target.getDistanceToPartition() / MEAN_VELOCITY_OF_MOVING
                        + mdw * target.getDistanceToMe() / MEAN_VELOCITY_OF_MOVING
                        - vsw * target.getVictimSituation();
                target.setCost(cost);
            }

        }

    }

    private void calculateDecisionParameters(Set<StandardEntity> victims, Map<EntityID, AmbulanceTarget> targetsMap) {
        AmbulanceTarget target;
        Human human;
        if (victims != null && !victims.isEmpty()) {
            for (StandardEntity victim : victims) {
                target = targetsMap.get(victim.getID());
                human = (Human) worldInfo.getEntity(victim.getID());
                    if (target == null) {
                        target = new AmbulanceTarget(victim.getID());

                        target.setPositionID(human.getPosition());

                        target.setDistanceToRefuge(worldInfo.getDistance(human.getPosition(), findNearestRefuge(human.getPosition())));

                        target.setDistanceToPartition(Util.distance(victim.getLocation(worldInfo.getRawWorld()), clusterCenterMap.get(this.clusterIndex)));
                    }
                target.setDistanceToMe(computingDistance(human));

                target.setVictimSituation(calculateVictimProfitability(human));

                targetsMap.put(victim.getID(), target);

            }
        }

    }


    private EntityID findNearestRefuge(EntityID positionId) {

        Collection<StandardEntity> refuges = worldInfo.getEntitiesOfType(REFUGE);
        EntityID nearestID = null;
        int nearestDistance = Integer.MAX_VALUE;
        int tempDistance;
        if (positionId != null && !refuges.isEmpty()) {

            for (StandardEntity refugeEntity : refuges) {
                tempDistance = worldInfo.getDistance(refugeEntity.getID(), positionId);
                if (tempDistance < nearestDistance) {
                    nearestDistance = tempDistance;
                    nearestID = refugeEntity.getID();
                }
            }

        }

        return nearestID;
    }

    private int computingDistance(Human human) {

        double coefficient = 1;
        if (human instanceof AmbulanceTeam) {
            coefficient = 0.90;
        } else if (human instanceof PoliceForce) {
            coefficient = 0.95;
        } else if (human instanceof FireBrigade) {
            coefficient = 0.97;
        }
        return (int) (coefficient * Util.distance(human.getPosition(worldInfo.getRawWorld()).getLocation(worldInfo.getRawWorld()), worldInfo.getLocation(agentInfo.me())));
    }

    private double calculateVictimProfitability(Human human) {
        for (StandardEntity next : worldInfo.getEntitiesOfType(
                StandardEntityURN.CIVILIAN
        )
        ) {
            Human h = (Human) next;
            if (agentInfo.getID() == h.getID()) {
                continue;
            }
            if (h.isHPDefined() && h.isBuriednessDefined() && h.isDamageDefined() && h.isPositionDefined() && h.getHP() > 0) {
                if (h.getBuriedness() <= 0) {
                    h.getDamage();
                }
            }

        }
        int ttd = (int) Math.ceil(human.getHP() / (double) human.getDamage() * 0.8); //a pessimistic time to death
        return 100 / (double) ((human.getBuriedness() * ttd) + 1);
    }

}

